/*
 * Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
 *
 * Bullet Continuous Collision Detection and Physics Library
 * Copyright (c) 2003-2008 Erwin Coumans  http://www.bulletphysics.com/
 *
 * This software is provided 'as-is', without any express or implied warranty.
 * In no event will the authors be held liable for any damages arising from
 * the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose, 
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
package org.usfirst.team3132.simulate;

import com.bulletphysics.util.ObjectArrayList;
import com.bulletphysics.collision.broadphase.BroadphaseInterface;
import com.bulletphysics.collision.broadphase.DbvtBroadphase;
import com.bulletphysics.collision.dispatch.CollisionDispatcher;
import com.bulletphysics.collision.dispatch.CollisionObject;
import com.bulletphysics.collision.dispatch.DefaultCollisionConfiguration;
import com.bulletphysics.collision.shapes.BoxShape;
import com.bulletphysics.collision.shapes.BvhTriangleMeshShape;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.collision.shapes.CompoundShape;
import com.bulletphysics.collision.shapes.CylinderShapeX;
import com.bulletphysics.collision.shapes.TriangleIndexVertexArray;
import com.bulletphysics.demos.opengl.DemoApplication;
import com.bulletphysics.demos.opengl.GLDebugDrawer;
import com.bulletphysics.demos.opengl.GLShapeDrawer;
import com.bulletphysics.demos.opengl.IGL;
import com.bulletphysics.demos.opengl.LWJGL;
import com.bulletphysics.dynamics.DiscreteDynamicsWorld;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.dynamics.constraintsolver.ConstraintSolver;
import com.bulletphysics.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
import com.bulletphysics.dynamics.vehicle.DefaultVehicleRaycaster;
import com.bulletphysics.dynamics.vehicle.RaycastVehicle;
import com.bulletphysics.dynamics.vehicle.VehicleRaycaster;
import com.bulletphysics.dynamics.vehicle.VehicleTuning;
import com.bulletphysics.dynamics.vehicle.WheelInfo;
import com.bulletphysics.linearmath.DebugDrawModes;
import com.bulletphysics.linearmath.Transform;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import javax.vecmath.Vector3f;
import org.lwjgl.LWJGLException;
import org.lwjgl.input.Keyboard;
import static com.bulletphysics.demos.opengl.IGL.*;

/**
 *
 * @author jezek2
 */
public class Simulate extends DemoApplication {

    public static final float PI = 3.14159265358979323846f;
    public static final float PI_2 = 1.57079632679489661923f;
    public static final float PI_4 = 0.785398163397448309616f;
    public static final float LIFT_EPS = 0.0000001f;

    private int rightIndex = 0;
    private int upIndex = 1;
    private int forwardIndex = 2;
    private final Vector3f wheelDirectionCS0 = new Vector3f(0, -1, 0);
    private final Vector3f wheelAxleCS = new Vector3f(-1, 0, 0);
    private final int numWheels = 8;

    private static final int maxProxies = 32766;
    private static final int maxOverlap = 65535;
    // RaycastVehicle is the interface for the constraint that implements the raycast vehicle
    // notice that for higher-quality slow-moving vehicles, another approach might be better
    // implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts
    private float gEngineForce = 0.f;
    private float defaultBreakingForce = 10.f;
    private float gBreakingForce = 10.f;
    private float leftWheelForce = 0f;
    private float leftWheelBrake = 10f;
    private float rightWheelForce = 0f;
    private float rightWheelBrake = 10f;
    private float maxEngineForce = 1000.f;//this should be engine/velocity dependent
    private float maxBreakingForce = 10.f;
    private float wheelRadius = 0.5f;
    private float wheelWidth = 0.1f;
    private float wheelFriction = 5;//1e30f;
    private float rollInfluence = 0.1f;//1.0f;
    private float suspensionRestLength = 0.5f;
    private static final float CUBE_HALF_EXTENTS = 1f;
    ////////////////////////////////////////////////////////////////////////////
    public RigidBody carChassis;
    public boolean useDefaultCamera;
    //----------------------------
    public ObjectArrayList<CollisionShape> collisionShapes = new ObjectArrayList<CollisionShape>();
    public BroadphaseInterface overlappingPairCache;
    public CollisionDispatcher dispatcher;
    public ConstraintSolver constraintSolver;
    public DefaultCollisionConfiguration collisionConfiguration;
    public TriangleIndexVertexArray indexVertexArrays;
    public ByteBuffer vertices;
    public VehicleTuning tuning = new VehicleTuning();
    public VehicleRaycaster vehicleRayCaster;
    public RaycastVehicle vehicle;
    public float cameraHeight = 4f;
    public float minCameraDistance = 3f;
    public float maxCameraDistance = 10f;

    public Simulate(IGL gl) {
        super(gl);
        cameraPosition.set(30, 30, 30);
        useDefaultCamera = false;
    }

    @Override
    public void clientMoveAndDisplay() {
        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT);

        {
            for (int wheelIndex = 0; wheelIndex < 6; wheelIndex++) {
                vehicle.applyEngineForce(leftWheelForce, wheelIndex);
                vehicle.setBrake(leftWheelBrake, wheelIndex);
                wheelIndex++;
                vehicle.applyEngineForce(rightWheelForce, wheelIndex);
                vehicle.setBrake(rightWheelBrake, wheelIndex);
            }
        }

        float dt = getDeltaTimeMicroseconds() * 0.000001f;

        if (dynamicsWorld != null) {
            // during idle mode, just run 1 simulation step maximum
            int maxSimSubSteps = idle ? 1 : 2;
            if (idle) {
                dt = 1f / 420f;
            }

            int numSimSteps = dynamicsWorld.stepSimulation(dt, maxSimSubSteps);
        }

        renderme();

        // optional but useful: debug drawing
        if (dynamicsWorld != null) {
            dynamicsWorld.debugDrawWorld();
        }
    }

    @Override
    public void clientResetScene() {
        Transform tr = new Transform();
        tr.setIdentity();
        carChassis.setCenterOfMassTransform(tr);
        carChassis.setLinearVelocity(new Vector3f(0, 0, 0));
        carChassis.setAngularVelocity(new Vector3f(0, 0, 0));
        dynamicsWorld.getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(carChassis.getBroadphaseHandle(), getDynamicsWorld().getDispatcher());
        if (vehicle != null) {
            vehicle.resetSuspension();
            for (int i = 0; i < vehicle.getNumWheels(); i++) {
                // synchronize the wheels with the (interpolated) chassis worldtransform
                vehicle.updateWheelTransform(i, true);
            }
        }
    }

    @Override
    public void displayCallback() {
        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT);

        renderme();

        // optional but useful: debug drawing
        if (dynamicsWorld != null) {
            dynamicsWorld.debugDrawWorld();
        }
    }

    ///a very basic camera following the vehicle
    @Override
    public void updateCamera() {
        if (useDefaultCamera) {
            super.updateCamera();
            return;
        }

        // //#define DISABLE_CAMERA 1
        //#ifdef DISABLE_CAMERA
        //DemoApplication::updateCamera();
        //return;
        //#endif //DISABLE_CAMERA

        gl.glMatrixMode(gl.GL_PROJECTION);
        gl.glLoadIdentity();

        Transform chassisWorldTrans = new Transform();

        // look at the vehicle
        carChassis.getMotionState().getWorldTransform(chassisWorldTrans);
        cameraTargetPosition.set(chassisWorldTrans.origin);

        // interpolate the camera height
        //#ifdef FORCE_ZAXIS_UP
        //m_cameraPosition[2] = (15.0*m_cameraPosition[2] + m_cameraTargetPosition[2] + m_cameraHeight)/16.0;
        //#else
        cameraPosition.y = (15.0f * cameraPosition.y + cameraTargetPosition.y + cameraHeight) / 16.0f;
        //#endif

        Vector3f camToObject = new Vector3f();
        camToObject.sub(cameraTargetPosition, cameraPosition);

        // keep distance between min and max distance
        float cameraDistance = camToObject.length();
        float correctionFactor = 0f;
        if (cameraDistance < minCameraDistance) {
            correctionFactor = 0.15f * (minCameraDistance - cameraDistance) / cameraDistance;
        }
        if (cameraDistance > maxCameraDistance) {
            correctionFactor = 0.15f * (maxCameraDistance - cameraDistance) / cameraDistance;
        }
        Vector3f tmp = new Vector3f();
        tmp.scale(correctionFactor, camToObject);
        cameraPosition.sub(tmp);

        // update OpenGL camera settings
        gl.glFrustum(-1.0, 1.0, -1.0, 1.0, 1.0, 10000.0);

        gl.glMatrixMode(IGL.GL_MODELVIEW);
        gl.glLoadIdentity();

        gl.gluLookAt(cameraPosition.x, cameraPosition.y, cameraPosition.z,
                cameraTargetPosition.x, cameraTargetPosition.y, cameraTargetPosition.z,
                cameraUp.x, cameraUp.y, cameraUp.z);
    }

    @Override
    public void specialKeyboard(int key, int x, int y, int modifiers) {
        if (key == Keyboard.KEY_END) {
            return;
        }

        switch (key) {
            case Keyboard.KEY_LEFT: {
                leftWheelForce = -maxEngineForce;
                leftWheelBrake = 0f;
                rightWheelForce = maxEngineForce;
                rightWheelBrake = 0f;
                break;
            }
            case Keyboard.KEY_RIGHT: {
                leftWheelForce = maxEngineForce;
                leftWheelBrake = 0f;
                rightWheelForce = -maxEngineForce;
                rightWheelBrake = 0f;
                break;
            }
            case Keyboard.KEY_UP: {
                leftWheelForce = maxEngineForce;
                leftWheelBrake = 0f;
                rightWheelForce = maxEngineForce;
                rightWheelBrake = 0f;
                break;
            }
            case Keyboard.KEY_DOWN: {
                leftWheelForce = -maxEngineForce;
                leftWheelBrake = 0f;
                rightWheelForce = -maxEngineForce;
                rightWheelBrake = 0f;
                break;
            }

            case Keyboard.KEY_F5:
                useDefaultCamera = !useDefaultCamera;
                break;
            default:
                super.specialKeyboard(key, x, y, modifiers);
                break;
        }
    }

    @Override
    public void specialKeyboardUp(int key, int x, int y, int modifiers) {
        switch (key) {
            case Keyboard.KEY_UP: {
                gEngineForce = 0.f;
                gBreakingForce = defaultBreakingForce;
                break;
            }
            case Keyboard.KEY_DOWN: {
                gEngineForce = 0.f;
                gBreakingForce = defaultBreakingForce;
                break;
            }
            case Keyboard.KEY_LEFT:
            case Keyboard.KEY_RIGHT: {
                break;
            }
            default:
                super.specialKeyboardUp(key, x, y, modifiers);
                break;
        }
    }

    @Override
    public void renderme() {
        updateCamera();

        CylinderShapeX wheelShape = new CylinderShapeX(new Vector3f(wheelWidth, wheelRadius, wheelRadius));
        Vector3f wheelColor = new Vector3f(1, 0, 0);

        Vector3f worldBoundsMin = new Vector3f(), worldBoundsMax = new Vector3f();
        getDynamicsWorld().getBroadphase().getBroadphaseAabb(worldBoundsMin, worldBoundsMax);

        for (int i = 0; i < vehicle.getNumWheels(); i++) {
            // synchronize the wheels with the (interpolated) chassis worldtransform
            vehicle.updateWheelTransform(i, true);
            // draw wheels (cylinders)
            Transform trans = vehicle.getWheelInfo(i).worldTransform;
            GLShapeDrawer.drawOpenGL(gl, trans, wheelShape, wheelColor, getDebugMode()/*,worldBoundsMin,worldBoundsMax*/);
        }

        super.renderme();

        if ((getDebugMode() & DebugDrawModes.NO_HELP_TEXT) == 0) {
            setOrthographicProjection();
            gl.glDisable(GL_LIGHTING);
            gl.glColor3f(0, 0, 0);

            drawString("SHIFT+Cursor Left/Right - rotate lift", 350, 20, TEXT_COLOR);
            drawString("SHIFT+Cursor UP/Down - move fork up/down", 350, 40, TEXT_COLOR);
            drawString("F5 - toggle camera mode", 350, 60, TEXT_COLOR);

            resetPerspectiveProjection();
            gl.glEnable(GL_LIGHTING);
        }
    }

    @Override
    public void initPhysics() {
        CollisionShape groundShape = new BoxShape(new Vector3f(50, 3, 50));
        collisionShapes.add(groundShape);
        collisionConfiguration = new DefaultCollisionConfiguration();
        dispatcher = new CollisionDispatcher(collisionConfiguration);
        overlappingPairCache = new DbvtBroadphase();
        constraintSolver = new SequentialImpulseConstraintSolver();
        dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, overlappingPairCache, constraintSolver, collisionConfiguration);

        Transform tr = new Transform();
        tr.setIdentity();

        final float TRIANGLE_SIZE = 20f;

        // create a triangle-mesh ground
        int vertStride = 3 * 4;
        int indexStride = 3 * 4;

        final int NUM_VERTS_X = 20;
        final int NUM_VERTS_Y = 20;
        final int totalVerts = NUM_VERTS_X * NUM_VERTS_Y;

        final int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);

        vertices = ByteBuffer.allocateDirect(totalVerts * vertStride).order(ByteOrder.nativeOrder());
        ByteBuffer gIndices = ByteBuffer.allocateDirect(totalTriangles * 3 * 4).order(ByteOrder.nativeOrder());

        for (int i = 0; i < NUM_VERTS_X; i++) {
            for (int j = 0; j < NUM_VERTS_Y; j++) {
                float wl = 0.2f;
                // height set to zero, but can also use curved landscape, just uncomment out the code
                float height = 0.f;//20.f*sinf(float(i)*wl)*cosf(float(j)*wl);
                //#ifdef FORCE_ZAXIS_UP
                //m_vertices[i+j*NUM_VERTS_X].setValue(
                //	(i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
                //	(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE,
                //	height
                //	);
                //#else
                int idx = (i + j * NUM_VERTS_X) * 3 * 4;
                vertices.putFloat(idx + 0 * 4, (i - NUM_VERTS_X * 0.5f) * TRIANGLE_SIZE);
                vertices.putFloat(idx + 1 * 4, height);
                vertices.putFloat(idx + 2 * 4, (j - NUM_VERTS_Y * 0.5f) * TRIANGLE_SIZE);
                //#endif
            }
        }

        //int index=0;
        for (int i = 0; i < NUM_VERTS_X - 1; i++) {
            for (int j = 0; j < NUM_VERTS_Y - 1; j++) {
                gIndices.putInt(j * NUM_VERTS_X + i);
                gIndices.putInt(j * NUM_VERTS_X + i + 1);
                gIndices.putInt((j + 1) * NUM_VERTS_X + i + 1);

                gIndices.putInt(j * NUM_VERTS_X + i);
                gIndices.putInt((j + 1) * NUM_VERTS_X + i + 1);
                gIndices.putInt((j + 1) * NUM_VERTS_X + i);
            }
        }
        gIndices.flip();

        indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
                gIndices,
                indexStride,
                totalVerts, vertices, vertStride);

        boolean useQuantizedAabbCompression = true;
        groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);

        tr.origin.set(0, -4.5f, 0);

        collisionShapes.add(groundShape);

        // create ground object
        localCreateRigidBody(0, tr, groundShape);

        CollisionShape chassisShape = new BoxShape(new Vector3f(2.f, 0.2f, 2.f));
        collisionShapes.add(chassisShape);

        CompoundShape compound = new CompoundShape();
        collisionShapes.add(compound);
        Transform localTrans = new Transform();
        localTrans.setIdentity();
        // localTrans effectively shifts the center of mass with respect to the chassis
        localTrans.origin.set(0, 1f, 0);
        //#endif

        compound.addChildShape(localTrans, chassisShape);

        tr.origin.set(0, 0.f, 0);

        carChassis = localCreateRigidBody(800, tr, compound);//chassisShape);
        //m_carChassis->setDamping(0.2,0.2);

        clientResetScene();

        // create vehicle
        {

            vehicleRayCaster = new DefaultVehicleRaycaster(dynamicsWorld);
            vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);

            // never deactivate the vehicle
            carChassis.setActivationState(CollisionObject.DISABLE_DEACTIVATION);

            dynamicsWorld.addVehicle(vehicle);

            float connectionHeight = 1.2f;

            boolean isFrontWheel = true;

            // choose coordinate system
            vehicle.setCoordinateSystem(rightIndex, upIndex, forwardIndex);

            Vector3f connectionPointCS0 = null;
            for (int i = 0; i < numWheels / 2; i++) {
                float distanceBetweenFrontAndBackAxials = 4f * CUBE_HALF_EXTENTS - 2.2f * wheelRadius;
                float distanceBetweenAxials = distanceBetweenFrontAndBackAxials / (numWheels / 2 - 1f);
                float zCoordinate = distanceBetweenFrontAndBackAxials / 2f - i * distanceBetweenAxials;
                System.out.println("zCoordinate = " + zCoordinate);
                connectionPointCS0 = new Vector3f(1.7f * CUBE_HALF_EXTENTS - (0.3f * wheelWidth),
                        connectionHeight,
                        zCoordinate);
//                        2f * CUBE_HALF_EXTENTS - 1.1f * wheelRadius);
                vehicle.addWheel(connectionPointCS0,
                        wheelDirectionCS0,
                        wheelAxleCS,
                        suspensionRestLength,
                        wheelRadius,
                        tuning,
                        isFrontWheel);

                connectionPointCS0 = new Vector3f(-1.7f * CUBE_HALF_EXTENTS + (0.3f * wheelWidth),
                        connectionHeight,
                        zCoordinate);
//                      2f * CUBE_HALF_EXTENTS - 1.1f * wheelRadius);
                vehicle.addWheel(connectionPointCS0,
                        wheelDirectionCS0,
                        wheelAxleCS,
                        suspensionRestLength,
                        wheelRadius,
                        tuning,
                        isFrontWheel);

                isFrontWheel = false;
            }
/*
 *          connectionPointCS0 = new Vector3f(-1.7f * CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 0f);
            vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            connectionPointCS0 = new Vector3f(1.7f * CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 0f);
            vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            connectionPointCS0 = new Vector3f(-1.7f * CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2f * CUBE_HALF_EXTENTS + 1.1f * wheelRadius);
            vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            connectionPointCS0 = new Vector3f(1.7f * CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2f * CUBE_HALF_EXTENTS + 1.1f * wheelRadius);
            vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
*/
            for (int i = 0; i < vehicle.getNumWheels(); i++) {
                WheelInfo wheel = vehicle.getWheelInfo(i);
                wheel.wheelsDampingRelaxation = 100000;
                wheel.wheelsDampingCompression = 0;
                wheel.frictionSlip = wheelFriction;
                wheel.rollInfluence = rollInfluence;
            }
        }

        setCameraDistance(26.f);
    }

    public static void main(String[] args) throws LWJGLException {
        System.out.println("Starting simulation");
        Simulate simulate = new Simulate(LWJGL.getGL());
        simulate.initPhysics();
        simulate.getDynamicsWorld().setDebugDrawer(new GLDebugDrawer(LWJGL.getGL()));

        LWJGL.main(args, 800, 600, "Robot Simulation", simulate);
        System.out.println("Simulation complete");
    }
}
